
#include "st1.hh"
sc_time zerodelay = SC_ZERO_TIME;

bool compareByKey(const l2pnode& a, const l2pnode& b) {
    return a.lca < b.lca;
}


void mergesort(l2pnode* s1, l2pnode* s2, int i1, int i2, l2pnode* s3) {
    int p1 = 0, p2=0, p3=0;
    while(1){
        if(s1[p1].lca>s2[p2].lca){
            s3[p3] = s2[p2];
            p2++;
        } else if(s1[p1].lca==s2[p2].lca){
            s1[p1].pca = s2[p2].pca;
            s3[p3] = s1[p1];
            p1++;
        } else {
            s3[p3] = s1[p1];
            p1++;
        }
        p3++;
        if(p1==i1) {memcpy(&s3[p3], &s2[p2], sizeof(l2pnode)*(i2-p2));break;}
        if(p2==i2) {memcpy(&s3[p3], &s1[p1], sizeof(l2pnode)*(i1-p1));break;}
    }

}

void ST1::st1_thread(void) { // a function for systemC simulation kernel, void inside () can be omitted
    std::cout << "st1 thread init" << std::endl;
    while(1){
        wait(10, SC_MS);
        // std::cout << "st1 hello" << std::endl;
    }
}

void ST1::st1_b_transport(tlm::tlm_generic_payload &trans, sc_core::sc_time &delay)
{
    
    tlm::tlm_command cmd = trans.get_command();
    uint32_t addr = trans.get_address();
    unsigned char *ptr = trans.get_data_ptr();
    unsigned int len = trans.get_data_length();

    l2pnode item = *(l2pnode *)ptr;
    // cout << item.lca <<"  "<< item.pca <<endl;
    trans.set_response_status(tlm::TLM_OK_RESPONSE);
    
    cmdset.write(item);
}

void ST1::l1_thread()
{
    l2pnode item;
    while(1){
        cmdset.read(item);
        for(int i=0;i<l1_loc;i++){
            if(l1[i].lca == item.lca) l1[i].pca = item.pca;
        }
        if(l1_loc<4) {
            l1[l1_loc] = item;
            l1_loc++;
        } else {

        }
        if(l1_loc == 4){
            l1_loc = 0;
            sort(l1, l1 + 4, compareByKey);
            // cout << l1[0].lca << "\t" << l1[1].lca<< "\t" << l1[2].lca<< "\t" << l1[3].lca<<endl;
            l1t2.trigger.notify();
        }
    }
}

void ST1::l2_thread()
{
    while(1){
        wait(l1t2.trigger);
        memcpy(l2[l2_all], l1, sizeof(l2pnode)*4);
        l2_all++;

        if(l2_all==4){
            l2_all = 0;
            l2t3.trigger.notify();
        }
    }
}

void ST1::l3_thread()
{
    while(1){
        wait(l2t3.trigger);
        l2pnode l3sort[16];
        l2pnode l3sort0[8], l3sort1[8];
        // cout<<"triggerd l3 layer"<<endl;
        mergesort(l2[0], l2[1], 4, 4, l3sort0);
        mergesort(l2[2], l2[3], 4, 4, l3sort1);
        mergesort(l3sort0, l3sort1, 8, 8, l3sort);
        // for(int i=0;i<16;i++) cout<<l3sort[i].lca<<endl;
        memcpy(l3[l3_all], l3sort, sizeof(l2pnode)*16);
        l3_all++;
        if(l3_all==4){
            l3_all = 0;
            l3t4.trigger.notify();
        }
    
    }
}

void ST1::l4_thread()
{
    while(1){
        wait(l3t4.trigger);
        l2pnode l4sort[64];
        l2pnode l4sort0[32], l4sort1[32];
        // cout<<"triggerd l4 layer"<<endl;
        mergesort(l3[0], l3[1], 16, 16, l4sort0);
        mergesort(l3[2], l3[3], 16, 16, l4sort1);
        mergesort(l4sort0, l4sort1, 32, 32, l4sort);
        // for(int i=0;i<16;i++) cout<<l3sort[i].lca<<endl;
        memcpy(l4[l4_all], l4sort, sizeof(l2pnode)*64);
        l4_all++;
        if(l4_all==4){
            l4_all = 0;
            l4t5.trigger.notify();
        }
    
    }

}

void ST1::l5_thread()
{
    while(1){
        wait(l4t5.trigger);
        l2pnode l5sort[256];
        l2pnode l5sort0[128], l5sort1[128];
        // cout<<"triggerd l5 layer"<<endl;
        mergesort(l4[0], l4[1], 64, 64, l5sort0);
        mergesort(l4[2], l4[3], 64, 64, l5sort1);
        mergesort(l5sort0, l5sort1, 128, 128, l5sort);
        // for(int i=0;i<16;i++) cout<<l3sort[i].lca<<endl;
        memcpy(l5[l5_all], l5sort, sizeof(l2pnode)*256);
        l5_all++;
        if(l5_all==4){
            l5_all = 0;
            l5tdsa.trigger.notify();
        }
    
    }
}


void ST1::dsa_thread()
{
    while(1){
        wait(l5tdsa.trigger);
        l2pnode dsasort[1024];
        l2pnode dsasort0[512], dsasort1[512];
        cout<<"triggerd dsa layer"<<endl;
        mergesort(l5[0], l5[1], 256, 256, dsasort0);
        mergesort(l5[2], l5[3], 256, 256, dsasort1);
        mergesort(dsasort0, dsasort1, 512, 512, dsasort);
        // for(int i=0;i<16;i++) cout<<l3sort[i].lca<<endl;
        memcpy(&dsa[dsa_index], dsasort, sizeof(l2pnode)*1024);
        dsa_index += 1024;
        if(dsa_index==8192){
            dsa_index = 0;
            sort(dsa, dsa + 8192, compareByKey);
            cout<<"dsa layer full"<<endl;

            vector<l2pnode> vec(dsa, dsa + 8192);
            int index=0;
            for(int i=0;i<8191;i++){
                if(vec[index].lca == vec[index+1].lca){
                    vec.erase(vec.begin()+index);
                    continue;
                }
                index++;
            }
            cout<<"size of dsa after remove duplications is:"<<vec.size()<<endl;
            copy(vec.begin(), vec.end(), dsa);
            // for(int i=0;i<16;i++) cout<<dsa[i].lca<<endl;

            //insert dsa to 
            tlm::tlm_generic_payload* trans = new tlm::tlm_generic_payload;
            trans->set_command( tlm::TLM_WRITE_COMMAND); 
            trans->set_address( 0x0 ); //date read in sbp
            trans->set_data_ptr( reinterpret_cast<unsigned char*>(dsa));
            trans->set_data_length( vec.size() );
            trans->set_streaming_width( vec.size() ); // = data_length to indicate no streaming
            trans->set_byte_enable_ptr( 0 ); // 0 indicates unused
            trans->set_dmi_allowed( false ); // Mandatory initial value
            trans->set_response_status( tlm::TLM_INCOMPLETE_RESPONSE ); // Mandatory
            st12st3_isocket->b_transport( *trans, zerodelay );  // Blocking transport call


        }
    
    }
}




// int sc_main(int, char*[]) { // entry point
// //   hello1(); // approach #1: manually invoke a normal function
//   ST1 ST1("st1"); // approach #2, instantiate a systemC module
//   sc_start(); // let systemC simulation kernel to invoke helloworld.hello2();
//   return 0;
// }
